使用矩阵表示状态

状态向量

你刚刚学会了如何使用移动模型来表示无人驾驶车的状态。事实证明，矩阵可以方便、简洁地表示车辆状态。

我们回顾一下恒定速度移动模型：

$distance = velocity \times time$

车辆的状态由两个变量表示：距离和速度。如果要在 Python 中存储这两个变量，我们可能会使用下面这样的列表：

state = [distance, velocity]

Python 代码看起来很像数学上的向量概念。一个向量本质上就是一个列表，列表中的每个元素都包含一些信息。

因此不难想象，一个状态向量可能包含更多信息。在二维世界里，状态可能包括 $distance_x$ 、 $distance_y$ 、 $velocity_x$ 以及 $velocity_y$ 。

在 Python 中，列表应该是下面这样的：

state = [distance_x, distance_y, velocity_x, velocity_y]

或者，还可以使用一个更复杂的模型，把车辆转向角度和转弯速度也包括在内：

state = [distance_x, distance_y, velocity_x, velocity_y, angle, angle_rate]

一维世界中的状态向量

现在，考虑只带有距离和速度的一维模型。

state = [distance, velocity]

当速度不变时，你是如何计算车辆的距离和速度的？有两个不同的方程。

$\begin{cases}distance = velocity \times time \\velocity = velocity\end{cases}$

现在，从状态角度考虑这些方程。出于方便，你可以把距离表示为 $x$ ，速度表示为 $v$ ，时间表示为 $t$ 。

初始状态

车辆刚开始移动时，你可以认为 $t = t_0$ ， $x = x_0$ ，并且 $v = v_0$ 。因此，状态向量为 $state_0 = [x_0, v_0]$ 。

第一步

过了一段时间后，现在时间是 $t_1$ ，状态如何？

在 $t_1$ ，状态向量是：

$state_1 = [x_1, v_1]$

根据模型公式，

$x_1 = x_0 + v_0 \times (t_1 - t_0)$

因为速度恒定：

$v_1 = v_0$ .

第二步

在第二步 $t_2$ 后，状态向量是：
$state_2 = [x_2, v_2]$

其中

$x_2 = x_1 + v_0 \times (t_2 - t_1)$

因为速度恒定：
$v_2 = v_0$ .

更好的方式

现在看来，数学也不难，对吗？我们有一个距离方程和一个速度方程。把前面的状态代入到每个方程中，随着时间变化，即可得到新的速度和新的距离。

但想象一下，如果你的无人驾驶车模型变得越来越复杂，会发生什么？如果我们必须考虑 x 方向、y 方向、x 速度和 y 速度、转向角和角速度时，会发生什么？或者，像无人驾驶飞机或直升机那样，模型更复杂，甚至包括 z 方向时，我们要怎么办？

我们无需逐个更新方程。我们可以用矢量和矩阵来完成所有的计算。

使用矩阵代数更新状态

如果你回顾一下更新距离的公式，你会注意到，距离取决于之前的距离、初始速度、以及自前一次距离公式更新经过了多长时间。因此，可以得出一个通用函数：

$x_{t+1} = x_{t} + v_0 \times (t_{t+1} - t_{t})$

$t_{t+1} - t_{t}$ 还可以写作：

$\Delta t$

对于恒定速度模型，通用速度方程为：

$v_{t+1} = v_{t}$

怎么才能把 x 和 v 方程组合成一个矩阵代数表达式？矩阵代数应该是下面这样的：

$\begin {bmatrix} x_{t+1} \\ v_{t+1} \end {bmatrix} =\begin {bmatrix} 1& \Delta t \\0&1 \end{bmatrix}\times\begin {bmatrix} x_{t} \\ v_{t} \end{bmatrix}$

如果你不确定这个表达式是什么意思，或者矩阵如何相乘，不要担心。这是我们本课要学习的内容。

符号

我们刚刚看到的矩阵代数方程实际上是卡尔曼滤波器更新方程的一部分。

传统上，矩阵运算是这样的：

$\begin {bmatrix} x_{t+1} \\ v_{t+1} \end {bmatrix} =\begin {bmatrix} 1& \Delta t \\0&1 \end{bmatrix}\times\begin {bmatrix} x_{t} \\ v_{t} \end{bmatrix}$

使用下面的卡尔曼滤波器符号表示：
$\mathbf{\hat{x} {k|k-1}} = \mathbf{F} \mathbf{\hat{x} {k-1|k-1}}$

其中
$\mathbf{\hat{x}}$ 表示状态向量， $\mathbf{F}$ 表示矩阵。

$\begin {bmatrix} 1& \Delta t \\0&1 \end{bmatrix}$

$\mathbf{k - 1}$ 是前一步， $\mathbf{k }$ 是当前步。

在课程下面的部分，你会了解为什么符号会包括 $\mathbf{k-1|k-1}$ and $\mathbf{k|k-1}$ 。

这种符号可能会有点混乱。例如，下面两者有什么区别：
$x$ 和 $\mathbf{\hat{x}}$ ？

常规的 $x$ 通常表示沿 x 轴的距离；而粗体的 $\mathbf{\hat{x}}$ 则表示一个向量。在这里讨论的一维例子中， $\mathbf{\hat{x}}$ 向量包含两个变量：沿 x 轴的距离和速度；因此
$\bold{\hat{x}} = \begin{bmatrix} x \\ v \end{bmatrix}$ 。

为什么要用大写的 $\mathbf{F}$ 而不是 $\mathbf{f}$ ？大写的粗体 $\mathbf{F}$ 告诉你这个变量是一个矩阵。

QUIZ QUESTION: :

匹配变量和类型

ANSWER CHOICES: